# -*- coding: utf-8 -*-
"""
Created on Fri Dec 17 16:19:54 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Basic_paras import *

# In[1] Long term MPC model
# States
def ode_ies_l(x_l, uc, uz, ud):
    dxdt = SX.zeros(Nx_l)
    
    SOC = x_l[0]
    mstc = x_l[1]
    tbr = x_l[2]
    
    Gff = uc[0]
    Gfm = uc[1]
    Nec = uc[2]
    Gstu = uc[3]
    Pba = uc[4]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    ins = ud[1]
    Qother = ud[2]
    
    Gst = zst*Gstu + (zst - 1)*Gstu
    Pmt = k_mt*Gfm
    Qab = k_ab*Pmt
    Qec = k_ec*Nec
    Qst = k_st*Gst
    Qsl = Qab + Qec + Qst
    
    dxdt[0] = k_ba*Pba
    dxdt[1] = -Gst
    dxdt[2] = (Ubr*(ta - tbr) - Qsl + Qother)/Cbr
    
    return dxdt


# Output
def out_ies_l(x_l, uc, uz, ud):
    y_alg = MX.zeros(Ny_l)
    
    SOC = x_l[0]
    mstc = x_l[1]
    tbr = x_l[2]
    
    Gff = uc[0]
    Gfm = uc[1]
    Nec = uc[2]
    Gstu = uc[3]
    Pba = uc[4]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    ins = ud[1]
    Qother = ud[2]
    
    Gall = zma*Gab0 + zec*Gec0 + Gstu
    Ppv = k_pv*ins
    Pfc = k_fc*Gff
    Pmt = k_mt*Gfm
    Qec = k_ec*Nec
    Pcp = k_cp*Qec
    Ppmp = k_pmp*Gall
    Psl = Ppv + Pfc + Pmt + Pba - Pcp - Ppmp
    
    y_alg[0] = Psl
    y_alg[1] = tbr
    
    return y_alg

# In[2] Formulate discrete time dynamics
x_sym_l = SX.sym('x_l', Nx_l)
uc_sym_l = SX.sym('uc_l', Nuc_l)
uz_sym_l = SX.sym('uz_l', Nuz_l)
ud_sym_l = SX.sym('ud_l', Nud_l)

ode_sym_l = ode_ies_l(x_sym_l, uc_sym_l, uz_sym_l, ud_sym_l)
args_l = {'x': x_sym_l, 'p': vertcat(uc_sym_l, uz_sym_l, ud_sym_l), 'ode': ode_sym_l}
opts_l = {'tf': Delta_l, 'regularity_check': True}
I_ode_l = integrator('I_ode_l', 'rk', args_l, opts_l)
